import cv2
import time
from peripherals import get_LASER
import numpy as np

cap = cv2.VideoCapture("/dev/video17")

cap.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc(*"MJPG"))
cap.set(cv2.CAP_PROP_FPS, 30)
mtx = np.array([[389.1188600763897, 0.0, 340.332634484495], [0.0, 390.66690938053046, 301.5605063606293], [0.0, 0.0, 1.0]])
dist = np.array([[-0.06710032864662062, -0.019528809318719448, -0.0005139028515977271, 0.0005799139558447469, -0.017234191187122435]])
# cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
# cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)

w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
new_camera_mtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))
print(w, h)

# laser = get_LASER()
# laser.write(True)

fps = cap.get(cv2.CAP_PROP_FPS)
print(fps)

def undistort_and_cut(img, mtx, dist, new_camera_mtx,roi):
    img = cv2.undistort(img, mtx, dist, None, new_camera_mtx)
    x,y,w,h = roi
    return img[y:y+h,x:x+w]


post = 0
while cap.isOpened():
	
	ref, frame = cap.read()
	frame = undistort_and_cut(frame, mtx, dist, new_camera_mtx, roi)
	# 518, 362
	# print(frame.shape)


	cx = 287
	cy = 220
	cv2.circle(frame, (cx,cy),4,(255,20,20),3)
	cv2.imshow("video", frame)

	now = time.time()
	dt = now - post
	if dt:
		fps = 1/dt
	# print('FPS:{:.3f}'.format(fps))
	post = time.time()
	
	
	c = cv2.waitKey(1) & 0xff
	if c == 27:
		# 释放所有窗口
		cv2.destroyAllWindows()
		# laser.write(False)
		break

